within ThreeD_MBS_Dynamics.Examples.Snakeboard;

model RollingWheelSet
  parameter SI.Acceleration g = 9.81;
  parameter SI.Acceleration[3] Gravity = {0,-g,0};
  parameter SI.Mass m_rod = 0.5;
  parameter SI.Mass m_disc = 0.5;
  parameter SI.Mass m = Rod1.m + 2*Disc1.m;
  parameter SI.Radius r1 = 0.5;
  parameter SI.Radius r2 = 0.1;
  parameter SI.Length L = 1.5;
  parameter SI.Length Wp = 1.2;
  parameter Real coef = 0.5;
  parameter Real SmallPar = 1; // Parameter of the wheelset simplification
  parameter Stiffness c = 1000;
  parameter Viscosity d = 500;
  parameter SI.AngularVelocity omega0 = 0;
  parameter SI.AngularVelocity omega1 = 0.3;
  parameter Real phi = 0.1;
  parameter SI.Position[3] r0 = {0,r1,0};
  parameter SI.Velocity[3] v0 = {-cos(phi),0,sin(phi)};
  parameter SI.Velocity[3] v1 = v0 + cross({0, omega1, 0}, 0.5*L*{sin(phi), 0, cos(phi)});
  parameter SI.Velocity[3] v2 = v0 + cross({0, omega1, 0}, -0.5*L*{sin(phi), 0, cos(phi)});
  parameter SI.AngularVelocity[3] om01 = {0, omega1, 0} - {0, 0, v1*{cos(phi), 0, -sin(phi)}/r1};
  SpringJoint Joint1(
    c = c,
    d = d,
    nA = {0, 0, 1},
    nB = {0, 0, 1},
    rA = {0, 0, 0},
    rB = {0, 0, 0.5*L}) annotation (extent=[-70,30; -30,70]);
  SpringJoint Joint2(
    c = c,
    d = d,
    nA = {0, 0, 1},
    nB = {0, 0, 1},
    rA = {0, 0, 0},
    rB = {0, 0, -0.5*L}) annotation (extent=[70,30; 30,70]);
  Disc Disc1(
    Gravity = Gravity,
    m = SmallPar*m_disc,
    r1 = r1,
    q(start = {cos(phi/2), 0, sin(phi/2), 0}),
    omega(start = {0, omega1, 0} - {0, 0, v1*{cos(phi),0,-sin(phi)}/r1}),
    v(start = v1),
    I = SmallPar*m_disc*[0.25*r1^2,         0,        0;
                                 0, 0.25*r1^2,        0;
                                 0,         0, 0.5*r1^2],
    r(start = r0 + 0.5*L*{sin(phi), 0, cos(phi)})) 
    annotation (extent=[-70,-20; -30,20], rotation=90);
  Disc Disc2(
    Gravity = Gravity,
    m = SmallPar*m_disc,
    r1 = r1,
    omega(start = {0, omega1, 0} - {0, 0, v2*{cos(phi), 0, -sin(phi)}/r1}),
    v(start = v2),
    q(start = {cos(phi/2), 0, sin(phi/2), 0}),
    I = SmallPar*m_disc*[0.25*r1^2,         0,        0;
                                 0, 0.25*r1^2,        0;
                                 0,         0, 0.5*r1^2],
    r(start = r0 - 0.5*L*{sin(phi), 0, cos(phi)})) 
    annotation (extent=[70,-20; 30,20], rotation=90);
  RollingDisc.Disc_on_Base Disc_on_Plane1(
    ri(start = {0, -r1, 0}),
    r(start = r0 + 0.5*L*{sin(phi), 0, cos(phi)} + {0, -r1, 0})) annotation (extent=[-70,-70; -30,-30], rotation=90);
  RollingDisc.Disc_on_Base Disc_on_Plane2(
    ri(start = {0, -r1, 0}),
    r(start = r0 - 0.5*L*{sin(phi), 0, cos(phi)} + {0, -r1, 0})) annotation (extent=[70,-70; 30,-30], rotation=90);
  Rod_1 Rod1(
    Gravity = Gravity,
    coef = coef,
    m = SmallPar*(m_rod + coef*m_rod),
    r2 = r2,
    L = L,
    Wp = Wp,
    q(start = {cos(phi/2), 0, sin(phi/2), 0}),
    I = m_rod/12*[L^2 + 3*r2^2 + 4*L^2*coef^3 + 3*coef*r2^2,                          0,                                   0;
                                                          0, L^2 + 3*r2^2 + 6*coef*r2^2,                                   0;
                                                          0,                          0, 6*r2^2 + 4*L^2*coef^3 + 3*coef*r2^2],
    v(start=v0),
    omega(start={0,omega1,omega0}),
    r(start=r0)) annotation (extent=[-20,30; 20,70]);
  annotation (
    Diagram,
    experiment(
      StopTime=10,
      NumberOfIntervals=5000,
      Tolerance=1e-012),
    experimentSetupOutput);
  WrenchPort InPortJoint annotation (extent=[-10, 80; 10, 100]);
  KinematicPort OutPortJoint annotation (extent=[20,-100; 40,-80]);
  KinematicPort OutPortBase annotation (extent=[-40,-100; -20,-80]);
  SI.Energy Ed;
equation
  Ed = Joint1.Ed + Joint2.Ed;
  connect(Joint1.OutPortB, Rod1.InPortJointA) 
    annotation (points=[-44,68; -44,76; -10,76; -10,68]);
  connect(Rod1.InPortJointB, Joint2.OutPortB) 
    annotation (points=[10,68; 10,76; 44,76; 44,68]);
  connect(Joint1.InPortB, Rod1.OutPort) 
    annotation (points=[-44,32; -44,26; 0,26; 0,32]);
  connect(Joint2.InPortB, Rod1.OutPort) 
    annotation (points=[44,32; 44,26; 0,26; 0,32]);
  connect(Rod1.InPortJoint, InPortJoint) annotation (points=[0,68; 0,90]);
  connect(Rod1.OutPort, OutPortJoint) 
    annotation (points=[0,32; 0,-76; 30,-76; 30,-90]);
  connect(Disc1.InPortJoint, Joint1.OutPortA) annotation (points=[-68,6;
        -76,6; -76,76; -56,76; -56,68], style(color=3, rgbcolor={0,0,255}));
  connect(Disc1.InPortRoll, Disc_on_Plane1.OutPortB) annotation (points=[-68,-6;
        -76,-6; -76,-44; -68,-44],         style(color=3, rgbcolor={0,0,
          255}));
  connect(Joint2.OutPortA, Disc2.InPortJoint) annotation (points=[56,68;
        56,76; 76,76; 76,6; 68,6], style(color=3, rgbcolor={0,0,255}));
  connect(Disc2.InPortRoll, Disc_on_Plane2.OutPortB) annotation (points=[68,-6;
        76,-6; 76,-44; 68,-44],        style(color=3, rgbcolor={0,0,255}));
  connect(Joint2.InPortA, Disc2.OutPort) annotation (points=[56,32; 56,22;
        20,22; 20,-1.10215e-015; 32,-1.10215e-015], style(color=3,
        rgbcolor={0,0,255}));
  connect(Joint1.InPortA, Disc1.OutPort) annotation (points=[-56,32; -56,
        22; -20,22; -20,-1.10215e-015; -32,-1.10215e-015], style(color=3,
        rgbcolor={0,0,255}));
  connect(Disc_on_Plane1.InPortA, OutPortBase) annotation (points=[-32,
        -56; -10,-56; -10,-90; -30,-90], style(color=3, rgbcolor={0,0,255}));
  connect(Disc_on_Plane2.InPortA, OutPortBase) annotation (points=[32,-56;
        10,-56; 10,-90; -30,-90], style(color=3, rgbcolor={0,0,255}));
  connect(Disc1.OutPort, Disc_on_Plane1.InPortB) annotation (points=[-32,
        -1.10215e-015; -26,-1.10215e-015; -26,0; -20,0; -20,-44; -32,-44],
      style(color=3, rgbcolor={0,0,255}));
  connect(Disc2.OutPort, Disc_on_Plane2.InPortB) annotation (points=[32,
        -1.10215e-015; 26,-1.10215e-015; 26,0; 20,0; 20,-44; 32,-44], style(
        color=3, rgbcolor={0,0,255}));
end RollingWheelSet;
